/**
 * @file pid.c
 * @brief PID控制器实现，用于电机速度环控制
 */

#include "pid.h"

/**
 * @brief 初始化PID控制器
 * 
 * @param pid PID控制器指针
 * @param Kp 比例系数
 * @param Ki 积分系数
 * @param Kd 微分系数
 * @param out_min 输出下限
 * @param out_max 输出上限
 */
void PID_Init(PID_t *pid, float Kp, float Ki, float Kd, float out_min, float out_max) {
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->target = 0.0f;
    pid->prev_error = 0.0f;
    pid->integral = 0.0f;
    pid->derivative = 0.0f;
    pid->output = 0.0f;
    pid->output_min = out_min;
    pid->output_max = out_max;
    pid->enabled = 1; // 默认启用
}

/**
 * @brief 设置PID目标值
 * 
 * @param pid PID控制器指针
 * @param target 目标值
 */
void PID_SetTarget(PID_t *pid, float target) {
    pid->target = target;
}

/**
 * @brief 计算PID输出
 * 
 * @param pid PID控制器指针
 * @param measured 当前测量值
 * @return float PID计算后的输出值
 */
float PID_Compute(PID_t *pid, float measured) {
    // 如果PID控制器被禁用，直接返回0
    if (!pid->enabled) {
        return 0.0f;
    }
    
    // 计算误差
    float error = pid->target - measured;
    
    // 计算积分项
    pid->integral += error;
    
    // 积分限幅，防止积分饱和
    if (pid->integral > pid->output_max) {
        pid->integral = pid->output_max;
    } else if (pid->integral < pid->output_min) {
        pid->integral = pid->output_min;
    }
    
    // 计算微分项
    pid->derivative = error - pid->prev_error;
    
    // 计算PID输出
    pid->output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
    
    // 输出限幅
    if (pid->output > pid->output_max) {
        pid->output = pid->output_max;
    } else if (pid->output < pid->output_min) {
        pid->output = pid->output_min;
    }
    
    // 保存当前误差，用于下次计算
    pid->prev_error = error;
    
    return pid->output;
}

/**
 * @brief 重置PID控制器
 * 
 * @param pid PID控制器指针
 */
void PID_Reset(PID_t *pid) {
    pid->prev_error = 0.0f;
    pid->integral = 0.0f;
    pid->derivative = 0.0f;
    pid->output = 0.0f;
}

/**
 * @brief 设置PID控制器参数
 * 
 * @param pid PID控制器指针
 * @param Kp 比例系数
 * @param Ki 积分系数
 * @param Kd 微分系数
 */
void PID_SetParams(PID_t *pid, float Kp, float Ki, float Kd) {
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
}

/**
 * @brief 启用PID控制器
 * 
 * @param pid PID控制器指针
 */
void PID_Enable(PID_t *pid) {
    pid->enabled = 1;
}

/**
 * @brief 禁用PID控制器
 * 
 * @param pid PID控制器指针
 */
void PID_Disable(PID_t *pid) {
    pid->enabled = 0;
} 